// DropletComponents.cpp

#include "DropletComponents.h"

extern uint16_t _activeDropletID;
extern bool _verbose;
extern sem_t _lock;
extern std::map<uint16_t, Droplets::DropletStateData *> _dStates;
extern std::deque<Droplets::Message> _msgList;

// ========================================================================= //
//                            --- Helper functions ---
// ========================================================================= //
void setMotors(float angle)
{
	DropletStateData *dState = _dStates[_activeDropletID];
	if(angle == 60)
	{
		dState->powMotor1 = 1;
		dState->powMotor2 = 0;
		dState->powMotor3 = 1;
	}
	else if(angle == 90)
	{
		dState->powMotor1 = 1;
		dState->powMotor2 = 1;
		dState->powMotor3 = 0;
	}
	else if(angle == 120)
	{
		dState->powMotor1 = 0;
		dState->powMotor2 = 1;
		dState->powMotor3 = 1;
	}
	else if(angle == 240)
	{
		dState->powMotor1 = -1;
		dState->powMotor2 = 0;
		dState->powMotor3 = -1;
	}
	else if(angle == 270)
	{
		dState->powMotor1 = -1;
		dState->powMotor2 = -1;
		dState->powMotor3 = 1;
	}
	else	// angle == 300
	{
		dState->powMotor1 = 0;
		dState->powMotor2 = -1;
		dState->powMotor3 = -1;
	}
}

float roundAngle(uint16_t angle)
{
	if(angle >= 360) angle %= 360;

	if(angle < 75) return 60;
	else if(angle >= 75 && angle < 105) return 90;
	else if(angle >= 105 && angle < 180) return 120;
	else if(angle >= 180 && angle < 255) return 240;
	else if(angle >= 255 && angle < 285) return 270;
	else return 300;
}

// ============================================================================

// ROBOT SETUP
void Droplets::InitDroplet()
{
	DropletStateData *dState = _dStates[_activeDropletID];
	
	// Seed the random number generator
	srand(time(NULL));

	dState->powCapacitor = 0;
}

void Droplets::SetupLEDs()
{
	DropletStateData *dState = _dStates[_activeDropletID];
	dState->rBt = 0;
	dState->gBt = 0;
	dState->bBt = 0;
}

void Droplets::SetupIRSensors()
{
	DropletStateData *dState = _dStates[_activeDropletID];
	dState->powIRTransmit = 0;
}

void Droplets::SetupRGBSensor()
{
	DropletStateData *dState = _dStates[_activeDropletID];
}
	
void Droplets::SetupComm(uint16_t robotID) // Setup communication
{
	_dStates[_activeDropletID]->robotID = robotID;
}

void Droplets::SetupMotors()
{
	DropletStateData *dState = _dStates[_activeDropletID];
	dState->tx = dState->x;
	dState->ty = dState->y;
	dState->ta = dState->a;
	dState->dx = 0;
	dState->dy = 0;
	dState->da = 0;
	dState->speed = 0;
	dState->moveDir = 0;

	dState->powMotor1 = 0;
	dState->powMotor2 = 0;
	dState->powMotor3 = 0;
	dState->speed = 0;
	dState->moving = false;
	dState->rotating = false;
}

// ROBOT SUBSYSTEM - Timer
	void SetupTimer(int16_t ms);
	void StartTimer();
	void StopTimer();

// ROBOT SUBSYSTEM - RGB LED and Sensors
void Droplets::GetRGBLED (VizColors color, uint8_t *bt)
{
	DropletStateData *dState = _dStates[_activeDropletID];
	switch(color)
	{
	case Droplets::RED :
		*bt = dState->rBt;
		break;
	case Droplets::GREEEN :
		*bt = dState->gBt;
		break;
	case Droplets::BLUE :
		*bt = dState->bBt;
		break;
	default :
		*bt = 0;
	}
}

void Droplets::SetRGBLED (VizColors color, uint8_t bt)
{
	DropletStateData *dState = _dStates[_activeDropletID];
	switch(color)
	{
	case Droplets::RED :
		dState->rBt = bt;
		break;
	case Droplets::GREEEN :
		dState->gBt = bt;
		break;
	case Droplets::BLUE :
		dState->bBt = bt;
		break;
	default :
		return;
	}
}
	
void Droplets::SenseRGB(VizColors color, uint16_t *bt)
{
	DropletStateData *dState = _dStates[_activeDropletID];
}


// ROBOT SUBSYSTEM - Motion
// May want getters...
void Droplets::Rotate(int16_t angle, uint8_t speed)	// angle in degrees
{
	DropletStateData *dState = _dStates[_activeDropletID];

	if(angle >= 360) angle %= 360;

	dState->ta = angle;

	// TODO : Fix this. The calculation for da is obviously wrong.
	dState->da = speed;

	if(angle < 0)
	{
		dState->powMotor1 = -1;
		dState->powMotor1 = -1;
		dState->powMotor1 = -1;
		dState->moving = false;
		dState->rotating = true;
	}
	else if(angle > 0)
	{
		dState->powMotor1 = 1;
		dState->powMotor1 = 1;
		dState->powMotor1 = 1;
		dState->moving = false;
		dState->rotating = true;
	}
	else	// angle = 0;
	{
		dState->powMotor1 = 0;
		dState->powMotor1 = 0;
		dState->powMotor1 = 0;
		dState->rotating = false;
	}
}
	
void Droplets::Move(uint16_t dir, uint8_t speed, uint8_t dur)
{
	DropletStateData *dState = _dStates[_activeDropletID];
		
	float x = dState->x;
	float y = dState->y;
	float fDur = static_cast<float>(dur);
	float rDir = roundAngle(dir);

	setMotors(rDir);

	rDir += dState->a;	// This accounts for the robot's orientation
	dState->dx = cos((float)(rDir * APPX_PI / 180)) * static_cast<float>(speed);
	dState->dy = sin((float)(rDir * APPX_PI / 180)) * static_cast<float>(speed);
	dState->tx = (dState->dx * fDur) + x;
	dState->ty = (dState->dy * fDur) + y;

	if(_verbose)
	{
		printf("Robot ID - %u started move in \n\t Direction %u\t from pos (%f, %f)\t \
				to pos (%f, %f)\t in steps (%f, %f)\n",
			dState->robotID, dir, 
			dState->x, dState->y, dState->tx, dState->ty, dState->dx, dState->dy);
	}

	dState->moveDir = rDir;
	dState->speed = speed;
	dState->rotating = false;
	dState->moving = true;
}


bool Droplets::IsMoving()
{
	return _dStates[_activeDropletID]->moving;
}

bool Droplets::IsRotating()
{
	return _dStates[_activeDropletID]->rotating;
}

// ROBOT SUBSYSTEM - Power
void Droplets::CapacitorPower(uint16_t *power)
{
	DropletStateData *dState = _dStates[_activeDropletID];
	*power = dState->powCapacitor;
}
	
void Droplets::SystemPower(Legs leg, int8_t *power)	// -1, 0, +1 - on power, ground, off power
{
	DropletStateData *dState = _dStates[_activeDropletID];
	switch(leg)
	{
	case Droplets::FRONT :
		*power = dState->powFrontLeg;
		break;
	case Droplets::REAR_LEFT :
		*power = dState->powRearLeftLeg;
		break;
	case Droplets::REAR_RIGHT :
		*power = dState->powRearRightLeg;
		break;
	default :
		*power = 0;
	}
}

// TODO : Implement	void Droplets::(*SystemPowerHandler)(void);

// ROBOT SUBSYSTEM - IR Sensors / Comm
void Droplets::SendPacket(uint8_t *data,
	uint8_t length,
	Direction_e dir, 
	uint8_t priority,
	uint8_t distance)	// distance in cm
{
	DropletStateData *dState = _dStates[_activeDropletID];
}

void Droplets::SenseIR(Direction_e dir, uint16_t *bt)
{
	DropletStateData *dState = _dStates[_activeDropletID];
}

// TODO : Implement	void Droplets::(*ReceivePacketHandler)(Message *msg);

// ROBOT HELPER FUNCTIONS
uint16_t Droplets::Random(uint16_t max)
{
	return static_cast<uint16_t>(rand() % max);
}